#!/usr/bin/python

"""
pyode-tutorial-3.py
This is an implementation of the PyODE tutorial 3 program using ode3d.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

Note: This python example follows the tutorial3.py program distributed with
      the PyODE library.

      The original PyODE tutorial can be found here:
          http://pyode.sourceforge.net/tutorials/tutorial3.html
"""

import random
from math import pi

from pandac.PandaModules import OdePlaneGeom, Mat4

import ode3d
import pvis

# create_box
def create_box(world, density, lx, ly, lz):
    """Create a box body and its corresponding geom."""

    # Create body
    body = ode3d.GDMBody(world)
    element = ode3d.GDMElement()
    # Assign every box to the same category and collide with that category.
    # By default GDM collision bitfields are set to not collide with each other.
    # This can be called before or after DefineBox()
    element.SetCollisionBitfields(1,1) # Category = 1^0, collide with 1^0
    element.DefineBox(density, lx, ly, lz)
    body.AddElement('Box', element)

    return body

# drop_object
def drop_object(world, bodies):
    """Drop an object into the scene."""

    body = create_box(world, 1000, 1.0, 0.2, 0.2)
    body.setPosition((random.gauss(0, 0.1), 3.0, random.gauss(0, 0.1)))
    
    # Rotate by a random angle about all three axes
    q = pvis.Quat(1, random.random(), random.random(), random.random())
    q.normalize()
    body.setQuaternion(q)

    bodies.append(body)

# explosion
def explosion(bodies, force):
    """Simulate an explosion.

    Every object is pushed away from the origin.
    The force is dependent on the objects distance from the origin.
    """
    for b in bodies:
        vPos = pvis.Vec3(b.getPosition())
        a = max(0, 60 * force * (1.0 - 0.2 * vPos.lengthSquared()))
        vl = pvis.Vec3(vPos.x / 4.0, vPos.y, vPos.z / 4.0)
        if vl.length() < 0.01: # Need to catch divide by near zero!
            mag = 0.01
        else:
            mag = vl.length()

        vF = vl * a / mag
        b.addForce(vF)

# pull
def pull(bodies, counter, force):
    """Pull the objects back to the origin.

    Every object will be pulled back to the origin.
    Every couple of frames there'll be a thrust upwards so that
    the objects won't stick to the ground all the time.
    """
    for b in bodies:
        vPos = pvis.Vec3(b.getPosition())
        if vPos.length() < 0.01: # Need to catch divide by near zero!
            mag = 0.01
        else:
            mag = vPos.length()

        vF = vPos * (-force / mag)
        b.addForce(vF)
        if counter % 60 == 0:
            b.addForce((0, 10000, 0))  # Give them an upwards bump every so often

# Create a ode3d world object
world = ode3d.World()
world.setGravity((0, -9.81, 0))

# Set the pvis scene so we won't have auto-scaling goofing us up.
#world.scene.autoscale = False
#world.scene.center = (0, 2.5, 0)
#world.scene.range = 4
#world.scene.background = (0.8, 0.8, 0.9)
# Create an initial camera position
pvis.base.disableMouse()
pvis.base.camera.setPos(10, 4, 0)
pvis.base.camera.lookAt(0, 1, 0) #@UndefinedVariable -- lookAt(), more Panda3d magic?
pvis.base.camera.setR(90) #@UndefinedVariable -- setR(), more Panda3d magic?

# Turn mouse control back on
mat=Mat4(pvis.base.camera.getMat()) #@UndefinedVariable -- getMat(), more Panda3d magic?
mat.invertInPlace()
pvis.base.mouseInterfaceNode.setMat(mat)
pvis.base.enableMouse()

# Create a plane geom which prevent the objects from falling forever
floor = OdePlaneGeom(world.space, 0, 1, 0, 0)

# Create a display object for the floor
floorDisp = pvis.box(size=(10,.0001, 10), color=(.2,.2,.2))

# A list with ODE bodies
bodies = []

# Simulation Setup
fps = 50
# Timesteps larger than 0.01 can lead to sim problems (due to contact forces)
# We set a target time step for the sim and then adjust parameters depending on
# the display frame rate since each pass of loop is one displayed frame 
dtTarget = 0.01 
steps = max(int(1.0 / fps / dtTarget + 0.5), 1)
dt = 1.0 / fps / steps
# countstp keeps the sim from gettting boring at low frame rates
# The counts in the conditionals were designed for 50fps
countstep = int(50/fps + 0.5)

pvis.rate(fps) # Try to keep the specified framerate

state = 0
counter = 0
objcount = 0
import time
oldt = 0
force = 5 / dt # The forces must get smaller for longer time steps or the sim blows up

def loop(task):
    global counter
    global state
    global objcount
    global dt
    global bodies
    global oldt
    global force

#    print 'Counter =', counter, ' State =', state, ' objcount =', objcount
    t = time.time()
    #print t - oldt
    oldt = t

    counter += countstep
        
    if state == 0:
        if counter >= 20:
            drop_object(world, bodies)
            counter = 0
            objcount += 1
        if objcount >= 30:
            state = 1
            counter = 0
    # State 1: Explosion and pulling back the objects
    elif state == 1:
        if counter >= 100 and counter < 100 + countstep:
            print 'explosion'
            explosion(bodies, force)
        if counter == 200:
            print 'pull'
        if counter >= 200:
            # We need a larger force with higher count steps because
            # We will have much fewer loops to get all the blocks back
            pull(bodies, counter, force * countstep)
        if counter >= 500:
            print 'restart'
            counter = 20

    # Simulate
    for i in range(steps):
        # Simulation step
        world.step(dt)

    for body in bodies:
        body.UpdateDisplay()

#    pvis.rate(fps) # Try to keep the specified framerate

    return task.cont

# Testing only
#while True:
#    loop()
#    t = time.time()
#    print t - oldt
#    oldt = t

#world.scene.range.x = 30
pvis.Run(loop)
